#include <mc_path_planner/path_smoother/five_point_gradient.h>


FivePointGradientDescent::FivePointGradientDescent(boost::shared_ptr<MapBase> map_handler_ptr)
  :SmoothBase(map_handler_ptr){

}


FivePointGradientDescent::~FivePointGradientDescent(){

}

// 路径平滑函数
void FivePointGradientDescent::smoothPath(vector<Point2f>& points_in, vector<Point2f>& points_out){
    points_out.clear();
    int iterations = 10;

    points_out = points_in;
    int n = points_in.size();
    if (n < 5) {
        // 如果路径点少于5个，无法进行五点平滑，直接返回
        return;
    }

    // P = -1/6 * (P-2 - 4P-1 + 6P - 4P1 + P2)
    float a = -1.0 / 6.0;

    // 进行多次迭代
    for (int iter = 0; iter < iterations; ++iter) {
        // 对路径中的每个点（从第2个到倒数第2个）
        for (int i = 2; i < n - 2; ++i) {
            Point2f prev_2_point = points_out[i - 2];
            Point2f prev_1_point = points_out[i - 1];
            Point2f curr_point = points_out[i];
            Point2f back_1_point = points_out[i + 1];
            Point2f back_2_point = points_out[i + 2];

            // 计算梯度
            Point2f cache_point = curr_point;
            cache_point.x += a * (prev_2_point.x - 4.0 * prev_1_point.x + 6.0 * curr_point.x - 4.0 * back_1_point.x + back_2_point.x);
            cache_point.y += a * (prev_2_point.y - 4.0 * prev_1_point.y + 6.0 * curr_point.y - 4.0 * back_1_point.y + back_2_point.y);

            // 判断是否存在障碍物
            Point2i pixel_point = mapHandlerPtr_->WorldToOpencvPixel<Point2f, Point2i>(cache_point);
            if(mapHandlerPtr_->ValueIsValid(pixel_point)){
                // 更新当前点位置
                points_out[i] = cache_point;
            }    
        }
    }
}